卡尔曼滤波

Kalman Filter
Optimal-Recursive-Data Processing-Algorithm 优化-递归-数据处理-算法

最优的线性滤波器，虽然名称为滤波器，但实际上也可属于状态观测器
态观测器（stateobserver），用来从输入和输出“重构”系统的状态。

生活中存在大量的不确定性，此算法得以普适地应用

不存在完美的数学模型
系统的扰动不可控，较难建模
测量传感器存在误差

引入例子

估计真实结果，取平均值
第 $k$ 次测量 $z_{k}$
第 $k$ 次的估计值 ${\hat{x}}_{k}$

\begin{aligned} {\hat{x}}_{k} & = \frac{1}{k} (z_{1} + z_{2} + \dots + z_{k}) \\ = \frac{1}{k} (z_{1} + z_{2} + \dots + z_{k - 1}) + \frac{1}{k} z_{k} \\ = \frac{k - 1}{k} \frac{1}{k - 1} (z_{1} + z_{2} + \dots + z_{k - 1}) + \frac{1}{k} z_{k} \\ = \frac{k - 1}{k} {\hat{x}}_{k - 1} + \frac{1}{k} z_{k} \\ = {\hat{x}}_{k - 1} + \frac{1}{k} (z_{k} - {\hat{x}}_{k - 1}) \end{aligned}

提炼出：

\begin{array}{r} {\hat{x}}_{k} = {\hat{x}}_{k - 1} + K_{k} (z_{k} - {\hat{x}}_{k - 1}) \end{array}

${\hat{x}}_{k}$ 当前估计值
${\hat{x}}_{k - 1}$ 上一次的估计值
$K_{k}$ Kalman Gain
$z_{k}$ 当前的测量值

递归，且只与上一次的有关联

\begin{array}{r} K_{k} = \frac{e_{E S T_{k - 1}}}{e_{E S T_{k - 1}} + e_{M E A_{k}}} \end{array}

$e_{E S T}$ 估计误差
$e_{M E A}$ 测量误差
测量方法或测量工具（传感器测量）的固定误差范围

{\begin{cases} e_{E S T_{k - 1}} ≫ e_{M E A_{k}} & K_{k} \to 1 & {\hat{x}}_{k} = z_{k} \\ e_{E S T_{k - 1}} ≪ e_{M E A_{k}} & K_{k} \to 0 & {\hat{x}}_{k} = {\hat{x}}_{k - 1} \end{cases}

步骤

$K_{k} = \frac{e_{E S T_{k - 1}}}{e_{E S T_{k - 1}} + e_{M E A_{k}}}$
${\hat{x}}_{k} = {\hat{x}}_{k - 1} + K_{k} (z_{k} - {\hat{x}}_{k - 1})$
$e_{E S T_{k}} = (1 - K_{k}) e_{E S T_{k - 1}}$

数学基础

数据融合

卡尔曼滤波器把不同来源的数据按不同的信噪比加权“整合”起来
Data Fusion
正态分布

$X_{1} \sim N (μ_{1}, σ_{1}^{2}), X_{2} \sim N (μ_{2}, σ_{2}^{2})$
$\hat{X} = X_{1} + K (X_{2} - X_{1})$

\hat{X} = {\begin{cases} X_{1} K = 0 \\ X_{2} K = 1 \end{cases}

求 $K$ 为何值时， $D (\hat{X})$ 最小

\begin{aligned} D (\hat{X}) & = (1 - K)^{2} σ_{1}^{2} + K^{2} σ_{2}^{2} \end{aligned}

$\frac{d D (\hat{X})}{d X} = 0$ 时， $K = \frac{σ_{1}^{2}}{σ_{1}^{2} + σ_{2}^{2}}$

估计的数据： $\hat{X} \sim N (\frac{μ_{1} σ_{2}^{2} + μ_{2} σ_{1}^{2}}{σ_{1}^{2} + σ_{2}^{2}}, \frac{σ_{1}^{2} σ_{2}^{2}}{σ_{1}^{2} + σ_{2}^{2}})$

协方差矩阵

核心公式

1.预测

状态空间
先验估计值：

\begin{array}{r} {\hat{X}}_{k^{-}} = A {\hat{X}}_{k - 1} + B u_{k - 1} \end{array}

$A$ 为状态矩阵
$B$ 为控制矩阵

先验误差的协方差矩阵：

\begin{array}{r} P_{k^{-}} = A P_{k - 1} A^{T} + Q \end{array}

$P_{k - 1}$ $k - 1$ 次的后验误差的协方差矩阵
$Q$ 过程误差的协方差矩阵

2.校正/更新

卡尔曼增益：

\begin{array}{r} K_{k} = \frac{P_{k^{-}} H^{T}}{H P_{k^{-}} H^{T} + R} \end{array}

后验估计值：

\begin{aligned} {\hat{X}}_{k} & = {\hat{X}}_{k^{-}} + K_{k} (Z_{k} - H {\hat{X}}_{k^{-}}) \end{aligned}

误差的协方差矩阵：

\begin{array}{r} P_{k} = (I - K_{k} H) P_{k^{-}} \end{array}

推导过程

\begin{aligned} X_{k} & = A X_{k - 1} + B u_{k - 1} + ω_{k - 1} \\ Z_{k} & = H X_{k} + ν_{k} \end{aligned}

$X_{k}$ 为状态变量
$A$ 为状态矩阵
$B$ 为控制矩阵
$u_{k}$ 为控制输入
$ω_{k - 1}$ Process Noise
过程噪音，计算中的不确定性
$P (ω) \sim (0, Q)$
$ν_{k}$ Measurement Noise
测量噪音，测量中的不确定性
$P (ν) \sim (0, R)$
自然界中的噪声的概率分布服从正态分布
期望为 0， $Q, R$ 均为协方差矩阵
以 $Q$ 为例，（ $R$ 的计算同理）：

$Q = E [ω ω^{T}] = E ([\begin{matrix} ω_{1} \\ ω_{2} \end{matrix}] [\begin{matrix} ω_{1} & ω_{2} \end{matrix}]) = E ([\begin{matrix} ω_{1}^{2} & ω_{1} ω_{2} \\ ω_{2} ω_{1} & ω_{2}^{2} \end{matrix}]) = [\begin{matrix} σ_{ω_{1}}^{2} & σ_{ω_{1}} σ_{ω_{2}} \\ σ_{ω_{2}} σ_{ω_{1}} & σ_{ω_{2}}^{2} \end{matrix}]$

由于存在噪声，而噪声无法被建模，直接通过状态空间方程得到两个不准确的值（估计值）：

计算出来的值先验估计值：
${\hat{X}}_{k^{-}} = A {\hat{X}}_{k - 1} + B u_{k - 1}$
测量出来的值：
${\hat{X}}_{k_{m e a}} = H^{- 1} Z_{k}$

卡尔曼滤波就是要根据两个不太准确的结果来得出准确的结果
结合数据融合，得到 ${\hat{X}}_{k}$ 后验估计值

\begin{aligned} {\hat{X}}_{k} & = {\hat{X}}_{k^{-}} + G (H^{- 1} Z_{k} - {\hat{X}}_{k^{-}}) \\ {\hat{X}}_{k} & = {\hat{X}}_{k^{-}} + K_{k} (Z_{k} - H {\hat{X}}_{k^{-}}) \end{aligned}

$G \in (0, 1) K_{k} = G H^{- 1} \in (0, H^{- 1})$

实际值 $X_{k}$ , 估计值 ${\hat{X}}_{k}$ , 误差 $e_{k} = X_{k} - {\hat{X}}_{k}$
$e_{k} \sim (0, P)$ $P = E (e e^{T}) = [\begin{matrix} σ_{e_{1}}^{2} & σ_{e_{1}} σ_{e_{2}} \\ σ_{e_{2}} σ_{e_{1}} & σ_{e_{2}}^{2} \end{matrix}]$

寻找 $K_{k}$ 使得误差协方差矩阵的迹最小 $t r (P) = σ_{e_{1}}^{2} + σ_{e_{2}}^{2}$ 有最小值

\begin{aligned} e_{k} & = X_{k} - {\hat{X}}_{k} \\ = X_{k} - ({\hat{X}}_{k^{-}} + K_{k} (Z_{k} - H {\hat{X}}_{k^{-}})) \\ = X_{k} - ({\hat{X}}_{k^{-}} + K_{k} (H X_{k} + ν_{k} - H {\hat{X}}_{k^{-}})) \\ = X_{k} - {\hat{X}}_{k^{-}} - K_{k} H X_{k} - K_{k} ν_{k} + K_{k} H {\hat{X}}_{k^{-}} \\ = X_{k} - {\hat{X}}_{k^{-}} - K_{k} H (X_{k} - {\hat{X}}_{k^{-}}) - K_{k} ν_{k} \\ = (I - K_{k} H) (X_{k} - {\hat{X}}_{k^{-}}) - K_{k} ν_{k} \\ = (I - K_{k} H) (e_{k^{-}}) - K_{k} ν_{k} \end{aligned}

$e_{k^{-}}$ 先验误差

卡尔曼增益：

\begin{array}{r} K_{k} = \frac{P_{k^{-}} H^{T}}{H P_{k^{-}} H^{T} + R} \end{array}

实际使用

实现多传感器的数据融合

理论

预测阶段：
状态预测：使用当前估计状态和系统模型来预测下一个状态。
协方差预测：估计预测状态的不确定性协方差。

更新阶段：
计算卡尔曼增益：基于预测状态的不确定性和实际测量的不确定性来计算。
更新估计：结合预测状态和实际测量来修正状态估计。
更新协方差：更新状态估计的不确定性。

线性动态系统的状态估计算法
一种数学优化技术，主要用于信号处理和控制理论中，以估计未知变量（通常是动态系统的状态）的最优估计。它是基于贝叶斯估计理论的线性滤波器，设计用来从一系列带有噪声的、不完全的观测数据中提取有用的信息。

它能够从一系列包含噪声的测量中估计动态系统的状态。这种滤波器由鲁道夫·卡尔曼（Rudolf E. Kalman）在 1960 年提出，它利用了状态空间模型和贝叶斯估计理论。卡尔曼滤波器的核心思想是使用系统的动态模型和观测数据，通过迭代的方式，对系统的状态进行最优估计。

在一个典型的场景中，卡尔曼滤波器会利用系统动态模型（通过状态方程描述）和观测数据（通过观测方程描述）来迭代地计算系统的状态。状态可以包括位置、速度、加速度等，而观测数据可能是传感器读数，比如雷达测距、GPS 信号等，这些数据往往受到噪声影响而包含误差。

卡尔曼滤波器的关键步骤包括预测（Predictor）和更新（Updater）两个阶段：

预测阶段：基于系统的运动学模型和前一时刻的最佳状态估计，预测当前状态及其协方差矩阵。
更新阶段：结合新的观测数据，通过计算卡尔曼增益，将预测结果与实际观测结果进行最优融合，得到更精确的状态估计。

卡尔曼滤波的优点在于它能够实时工作，每次接收到新的观测数据时都会更新并优化其内部的估计。这种方法考虑了观测噪声和过程噪声的统计特性，力求在统计意义下达到最小均方误差的估计效果。

卡尔曼滤波广泛应用于多个领域，包括但不限于航空航天导航、自动驾驶、机器人定位、信号处理、金融建模、天气预报等。

卡尔曼滤波器的基本工作原理包括两个步骤：预测（Predict）和更新（Update）。在预测阶段，滤波器根据系统的动态模型预测下一状态的可能性；在更新阶段，滤波器利用新的观测数据来更新预测，从而获得更准确的估计。

卡尔曼滤波器的关键优势在于其能够处理系统内部和外部的不确定性，如噪声和干扰。它适用于广泛的应用场景，包括航空航天、导航系统、信号处理、机器人定位、经济时间序列分析等。

在实际应用中，卡尔曼滤波器需要根据系统的具体情况来设计，包括确定系统的状态转移矩阵、观测矩阵、过程噪声协方差和观测噪声协方差等参数。此外，卡尔曼滤波器还有多种变体，如扩展卡尔曼滤波器（EKF）、无迹卡尔曼滤波器（UKF）和粒子滤波器（PF），它们适用于不同类型的非线性系统。

卡尔曼滤波器的实现通常需要一定的数学背景，包括线性代数和概率统计知识。在工程实践中，卡尔曼滤波器可以通过编程语言如MATLAB、Python等进行实现。